#ifndef SENSOR_ULTRA_HPP_
#define SENSOR_ULTRA_HPP_

#include <ros/ros.h>
#include <ros/time.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>

class Sensor_Ultra
{
private:
  //超声 波雷达设定命令
  uint8_t Mode[5] = { 0x00, 0x00, 0x00, 0x00, 0x00 };
  //超声波雷达参数获取命令
  uint8_t fdback[6] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };

  //超声波扫描数据
  uint8_t ScanBuff[5] = { 0x00, 0x00, 0x00, 0x00, 0x00 };
  std::vector<uint8_t> reciveData;

  int m_alarmDistance;
  uint16_t act_distance;
  uint16_t act_amp;
  uint16_t act_angle;

public:
  serial::Serial ser;
  ros::NodeHandle nh;
  ros::Subscriber MachineStatus_sub;

public:
  Sensor_Ultra();
  ~Sensor_Ultra();

  //向雷达设置模式命令
  void set_CommandMode(int type, int Var);
  //设置安全距离
  void set_AlarmDistance(int Dist);
  void setScanData(std::vector<uint8_t> data);
  void UltraRadScan_Start();
  bool Init_UltraRadComPort(std::string name);
  uint16_t comb16(uint8_t low, uint8_t high);
  void get_UltraRadComScanData();
  uint16_t get_ActualDistance();
  uint16_t get_ActualAmplitude();
  uint16_t get_ActualAngle();
  bool ObstacleScan();
  void CloseComPort();
  uint8_t DataOXROperate(uint8_t* pt, int num);
};

#endif